/*
 * @Author: Mcfly
 * @Date: 2021-07-14 20:30:00
 * @LastEditors: Mcfly
 * @Description: 
 * @FilePath: \FTTfly\LowerDriver\ti_system.c
 * 本人github:https://github.com/McflyWZX
 */
#include "stdbool.h"
#include "stdint.h"
#include "string.h"
#include "stdio.h"
#include <math.h>
#include "hw_ints.h"
#include "hw_memmap.h"
#include "fpu.h"
#include "gpio.h"
#include "pin_map.h"
#include "pwm.h"
#include "sysctl.h"
#include "uart.h"
#include "interrupt.h"
#include "timer.h"
#include "hw_gpio.h"
#include "WP_DataType.h"

#include "ti_system.h"
#include "Time_Cnt.h"
#include "Time.h"
#include "Bling.h"
#include "Usart.h"
#include "OLED.h"
#include "myiic.h"
#include "mpu6050.h"
#include "WP_PWM.h"
#include "Atti.h"
#include "VirtualOscTx.h"
#include "Throttle.h"
#include "AttiCtrl.h"
#include "PPM.h"
#include "NLinkTOF.h"
#include "HeightCtrl.h"
#include "Height.h"
#include "XY_Ctrl.h"
#include "OpenMV.h"
#include "RoboHand.h"
#include "ButterWorth.h"
#include "Laser.h"

#define SIGN(X) ((X) > 0 ? 1 : -1)

void TIMER1A_Handler(void);

void System_Start_Init(void)
{
    SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
    FPUEnable();
    FPULazyStackingEnable();
    initTime();
    //EEPROM_Init();
    //全局中断使能
    IntMasterEnable();
    IntPriorityGroupingSet(3);
    //	EEPROMMassErase();
}

ButterParameter *butterWorthGyro = NULL;
ButterParameter *butterWorthAccel = NULL;
ButterParameter *butterWorthPitch = NULL;
ButterBufferData butterWorthGyro_Buf[3] = {{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}};
ButterBufferData butterWorthGyroYAW2_Buf = {0.0f, 0.0f, 0.0f};
ButterBufferData butterWorthAccel_Buf[3] = {{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}};
ButterBufferData butterWorthPitch_Buf = {0.0f, 0.0f, 0.0f};

AttiCtrl *aCtrl = NULL;
Atti demoAtti;
NLinkTOF *buttomToF = NULL;
NLinkTOF *frontToF = NULL;
HuiMV *buttomMV = NULL;
EyeMV *eyeMV = NULL;
HeightCtrl *hCtrl = NULL;
Height *height = NULL;
XY_Ctrl *xyCtrl = NULL;
RoboHand *heightHand = NULL;
RoboHand *throttleHand = NULL;
RoboHand *rockerHandX = NULL;
RoboHand *rockerHandY = NULL;
FlowYTVG *flowYTVG = NULL;
Laser *laser = NULL;
void ti_systemInit(void)
{
    System_Start_Init();
    initTime();
    butterWorthGyro = ButterWorth_Init(200.0, 35);
    butterWorthAccel = ButterWorth_Init(200.0, 25);
    butterWorthPitch = ButterWorth_Init(200.0, 12);
    initVOT(1, 4);
    //ADC_Init();
    OLED_Init();
    display_6_8_string(0, 0, (unsigned char *)"Hello FTTfly");
    Init_I2C();
    while (MPU6050_Init())
        ; //MPU6050陀螺仪、加速度计配置初始化
    Atti_Init();
    Throttle_Init(0);
    memset(&demoAtti, 0x00, sizeof(Atti));
    demoAtti.pitch = 0;
    demoAtti.roll = 0;
    demoAtti.yaw = 0;
    aCtrl = AttiCtrl_Init();
    xyCtrl = XY_Ctrl_Init();
    Init_PWM();
    Bling_Init();
    ConfigureUART1();
    PPM_Init();
    hCtrl = HeightCtrl_Init();
    Time_init(0); //飞控调度定时器初始化     TODO:y
    buttomToF = TIMER2A_GetNLinkTOF();
    frontToF = TIMER2A_GetNLinkTOF2();
    buttomMV = TIMER2A_GetHuiMV();
    eyeMV = TIMER2A_GetEyeMV();
    height = TIMER2A_GetHeight();
    flowYTVG = FlowYTVG_Init();
    ConfigureUART2(19200, flowYTVG);
    heightHand = RoboHand_Init(0, 0, 0.25, 1);
    throttleHand = RoboHand_Init(0, 1400, 600, 1);
    rockerHandX = RoboHand_Init(0, 0.0f, 4.0f, 1);
    rockerHandY = RoboHand_Init(0, 0.0f, 4.0f, 1);
    laser = Laser_Init(SYSCTL_PERIPH_GPIOB, GPIO_PORTB_BASE, GPIO_PIN_3, 0.005f, 100.0f, 0.4f);
    laser->enable = 1;
    //防止爆堆，对象没内存。
    while(aCtrl == 0);
    while(buttomToF == 0);
    while(frontToF == 0);
    while(buttomMV == 0);
    while(eyeMV == 0);
    while(hCtrl == 0);
    while(height == 0);
    while(xyCtrl == 0);
    while(heightHand == 0);
    while(throttleHand == 0);
    while(rockerHandX == 0);
    while(rockerHandY == 0);
    while(flowYTVG == 0);
    while(butterWorthGyro == 0);
    while(butterWorthAccel == 0);
    while(butterWorthPitch == 0);
    while(laser == 0);
    delay_ms(200); //初始化延时	     TODO:y
}

//角度误差整定
bool startRecord = 0;
unsigned int recordLen = 0;
unsigned int totalRecordLen = 10000;
double sumAngle[2] = {0.0f, 0.0f};

PPM *demoPPM = NULL;
IMU_Read imuRead;
bool startLockHeight = 0; //是否开启定高，在0.07m前关闭定高
uint8_t runningState = 0; //指示飞行器的运行状态,1起飞，2飞行中，3开始降落，4已经降落
float midThrottle;
float landHeight = 0.08f;
float lastA[2][3];

//喷赛农药，地图相关
Vector2f pointA = {2.1f, -11.0f};
Vector2f aroundA = {-0.5f, -11.0f};
Vector2f pointG18 = {11.4f, -11.7f};
Vector2f G[] = {
//                                         理论值
    {2.8f, -11.7f},     //A	       	0	(2, -11)	[G21: (3, -12)]
    {21.0f, -11.7f},    //G15       1	(21, -12)
    {20.5f, -15.0f},    //G22       2	(21, -15)
    {2.3f, -14.4f},     //G28       3	(3, -15)
	
	{20.8f, -8.8f},    	//G11       4	(21, -9)
	{20.8f, -0.2f},		//G1		5	(21, 0)
	{17.4f, -0.4f},		//G2		6	(18, 0)
	{17.4f, -9.0f},		//G12		7	(18, -9)
	{14.4f, -9.0f},		//G13		8	(15, -9)
	{14.4f, 0.2f},		//G3		9	(15, 0)
	{11.4f, -0.1f},		//G4		10	(12, 0)
	{11.5f, -9.4f},     //G14       11	(12, -9)
};
int nG[] = {7, 7, 4, 4, 4, 4};

//自动驾驶相关
enum PilotState
{
    stayHome, 
    comeToPointA, 
    goAroundA, 
    findTower,
    readCode,
    backHomeG18,
    backHomeAroundA,
    goPath,
    landing,
    landed
};
float keepHeight = 1.4f;
unsigned int timeCount = 0;
float startLoop = 0.0f;
enum PilotState pilotState = stayHome;
bool stayPointOK = 0;
uint8_t nowGi = 0;
bool workingState = 0;
bool gridInX = 1;
int8_t nGrid = 0;
int8_t passingI = 0;
float xPreG = 0;
float yPreG = 0;
int8_t forceSpeedX = 0;
int8_t forceSpeedY = 0;
//extern float omegaOut[3];
void changeToState(enum PilotState s)
{
    switch(s)
    {
        case comeToPointA:
            pilotState = comeToPointA;
            RoboHand_SetVarEnd(rockerHandX, pointA.x);
            RoboHand_SetVarEnd(rockerHandY, pointA.y);
            timeCount = 0;
            stayPointOK = 0;
            break;

        case goPath:
            pilotState = goPath;
            timeCount = 0;
            stayPointOK = 0;
            workingState = 0;
            break;

        case readCode:
            pilotState = readCode;
            //TODO: 位置控制
            timeCount = 0;
            stayPointOK = 0;
            break;

        case backHomeG18:
            pilotState = backHomeG18;
            RoboHand_SetVarEnd(rockerHandX, pointG18.x);
            RoboHand_SetVarEnd(rockerHandY, pointG18.y);
            timeCount = 0;
            stayPointOK = 0;
            break;

        case backHomeAroundA:
            pilotState = backHomeAroundA;
            RoboHand_SetVarEnd(rockerHandX, aroundA.x);
            RoboHand_SetVarEnd(rockerHandY, aroundA.y);
            timeCount = 0;
            stayPointOK = 0;
            break;
		
		case goAroundA:
            pilotState = goAroundA;
            RoboHand_SetVarEnd(rockerHandX, aroundA.x);
            RoboHand_SetVarEnd(rockerHandY, aroundA.y);
            timeCount = 0;
            stayPointOK = 0;
            break;

        case findTower:
            pilotState = findTower;
            RoboHand_SetVarEnd(rockerHandX, G[10].x);
            RoboHand_SetVarEnd(rockerHandY, G[10].y);
            timeCount = 0;
            stayPointOK = 0;
            break;

        case landing:
            pilotState = landing;
            RoboHand_SetVarEnd(rockerHandX, -0.55f);
            RoboHand_SetVarEnd(rockerHandY, 0.55f);
            timeCount = 0;
            stayPointOK = 0;
            demoPPM->data[4] = 1000;
            nowGi = 0;
            break;
    }
}
void ti_system_Loop(void)
{
    MPU6050_Read_Data(&imuRead.gyro, &imuRead.accel, &imuRead.temperature);
    //滤波器滤波
    imuRead.gyro.x = ButterWorth_Work_LPB(imuRead.gyro.x, &butterWorthGyro_Buf[PARAM_PITCH], butterWorthGyro);
    imuRead.gyro.y = ButterWorth_Work_LPB(imuRead.gyro.y, &butterWorthGyro_Buf[PARAM_ROLL], butterWorthGyro);
    imuRead.gyro.z = ButterWorth_Work_LPB(imuRead.gyro.z, &butterWorthGyro_Buf[PARAM_YAW], butterWorthGyro);
	//imuRead.gyro.z = ButterWorth_Work_LPB(imuRead.gyro.z, &butterWorthGyroYAW2_Buf, butterWorthGyro);
    imuRead.accel.x = ButterWorth_Work_LPB(imuRead.accel.x, &butterWorthAccel_Buf[PARAM_ROLL], butterWorthAccel);
    imuRead.accel.y = ButterWorth_Work_LPB(imuRead.accel.y, &butterWorthAccel_Buf[PARAM_PITCH], butterWorthAccel);
    imuRead.accel.z = ButterWorth_Work_LPB(imuRead.accel.z, &butterWorthAccel_Buf[PARAM_YAW], butterWorthAccel);
    Atti nowAtti = Atti_Update(&imuRead.gyro, &imuRead.accel);
	nowAtti.pitch += 0.476683f;
	nowAtti.roll += 0.231269f;
    /***************************遥控器***************************/
    demoPPM = PPM_getData();
    //demoAtti.w = (demoPPM->data[2] - 1010) * 2.0f;
	float expDy = (demoPPM->data[1] - 1500) / 500.0f * 10.0f;
    float expDx = -(demoPPM->data[0] - 1500) / 500.0f * 10.0f;
    demoAtti.pitch = 0;
    demoAtti.roll = 0;
    demoAtti.gz = -(demoPPM->data[3] - 1500) / 500.0f * 10.0f / 200.0f;
    //遥控器
    //高度更新
    Height_UpdateFrame(height, &nowAtti);
    /***************************XY控制与自动驾驶***************************/
    //自动起降绕圈任务, 200Hz、0.005s、200个一秒
    if(demoPPM->data[4] > 1500 && height->h >= keepHeight - 0.15f)
    {
        //起飞
        switch(pilotState)
        {
            case stayHome:
				HuiMV_SetFun(buttomMV, 0xAA);
                if(timeCount >= 1 * 200 && buttomMV->option == 1)
                {
                    xyCtrl->OptPos.x = 0.03f * 0.5f * buttomMV->x + 0.5f * xyCtrl->OptPos.x;
                    xyCtrl->OptPos.y = -0.026f * 0.5f * buttomMV->y + 0.5f * xyCtrl->OptPos.y;
                    buttomMV->option = 0;
                }
                if(timeCount >= 2 * 200)
                {
                    nowGi = 0;
                    if(demoPPM->data[5] > 1750)
                    {
                        changeToState(goAroundA);
                    } else {
                        changeToState(comeToPointA);
                    }
                } else {
                    timeCount++;
                }
            break;

            //高级功能时，绕过A点，防止撞到杆
            case goAroundA:
                if(fabsf(xyCtrl->OptPos.x - aroundA.x) < 0.8f && fabsf(xyCtrl->OptPos.y - aroundA.y) < 0.8f)
                    stayPointOK = 1;
                if(timeCount >= 1 * 200)
                {
                    changeToState(comeToPointA);
                } else if(stayPointOK) {
                    timeCount++;
                }
            break;

            //到A点修正坐标
            case comeToPointA:
				HuiMV_SetFun(buttomMV, 0xAA);
                if(fabsf(xyCtrl->OptPos.x - pointA.x) < 0.8f && fabsf(xyCtrl->OptPos.y - pointA.y) < 0.8f)
                    stayPointOK = 1;
                if(timeCount >= 2 * 200 && buttomMV->option == 1 && stayPointOK && demoPPM->data[5] > 1250)
                {
                    xyCtrl->OptPos.x = 0.5f * (pointA.x + 0.03f * buttomMV->x) + 0.5f * xyCtrl->OptPos.x;
                    xyCtrl->OptPos.y = 0.5f * (pointA.y -0.026f * buttomMV->y) + 0.5f * xyCtrl->OptPos.y;
                    getPlaneAtti()->yaw = 0.2f * buttomMV->angle + 0.8f * getPlaneAtti()->yaw;
                    buttomMV->option = 0;
                }
                if(timeCount >= 3 * 200)
                {
                    changeToState(goPath);
                } else if(stayPointOK) {
                    timeCount++;
                }
            break;

            //找到塔杆，待编写
            case findTower:
                forceSpeedY = 1;
                if(fabsf(xyCtrl->OptPos.x - G[10].x) < 1.2f && fabsf(xyCtrl->OptPos.y - G[10].y) < 1.2f)
                    stayPointOK = 1;
                //如果找到了杆，读取条码
                // if(){
                //     changeToState(readCode);
                // }
                //如果到了G1过了三秒还没找到杆，那就回家
                if(timeCount >= 3 * 200)
                {
					forceSpeedY = 0;
                    changeToState(backHomeG18);
                } else if(stayPointOK) {
                    timeCount++;
                }
            break;

            case readCode:

            break;

            case backHomeG18:
                if(fabsf(xyCtrl->OptPos.x - pointG18.x) < 1.2f && fabsf(xyCtrl->OptPos.y - pointG18.y) < 1.2f)
                    stayPointOK = 1;
                if(timeCount >= 3 * 200)
                {
                    changeToState(backHomeAroundA);
                } else if(stayPointOK) {
                    timeCount++;
                }
            break;

            case backHomeAroundA:
                if(fabsf(xyCtrl->OptPos.x - aroundA.x) < 1.2f && fabsf(xyCtrl->OptPos.y - aroundA.y) < 1.2f)
                    stayPointOK = 1;
                if(timeCount >= 3 * 200)
                {
                    changeToState(landing);
                } else if(stayPointOK) {
                    timeCount++;
                }
            break;

            case landing:
				HuiMV_SetFun(buttomMV, 0xAA);
                if(fabsf(xyCtrl->OptPos.x - 0) < 1.2f && fabsf(xyCtrl->OptPos.y - 0) < 1.2f)
                    stayPointOK = 1;
                if(stayPointOK && buttomMV->option == 1)
                {
                    xyCtrl->OptPos.x = 0.03f * 0.6f * buttomMV->x + 0.4f * xyCtrl->OptPos.x;
                    xyCtrl->OptPos.y = -0.026f * 0.6f * buttomMV->y + 0.4f * xyCtrl->OptPos.y;
                    buttomMV->option = 0;
                }
                if(stayPointOK && timeCount >= 4 * 200)
                {
                    pilotState = landed;
                    RoboHand_SetVarEnd(rockerHandX, 0);
                    RoboHand_SetVarEnd(rockerHandY, 0);
                    // timeCount = 0;
                    stayPointOK = 1;
                    demoPPM->data[4] = 1000;
                } else if(stayPointOK) {
                    timeCount++;
                }
            break;

            //播撒工作
            case goPath:
				HuiMV_SetFun(buttomMV, 0xBB);
                RoboHand_SetVarEnd(rockerHandX, G[nowGi + workingState].x);
                RoboHand_SetVarEnd(rockerHandY, G[nowGi + workingState].y);
                if(fabsf(xyCtrl->OptPos.x - G[nowGi + workingState].x) < 1.2f && fabsf(xyCtrl->OptPos.y - G[nowGi + workingState].y) < 1.2f)
                    stayPointOK = 1;
                //进行格点播撒工作
                if(workingState)
                {
                    if(gridInX)
                    {
                        forceSpeedX = SIGN(G[nowGi + 1].x - G[nowGi].x);
                        forceSpeedY = 0;
                        if(stayPointOK)
                            forceSpeedX = 0;
                        xPreG = (G[nowGi + 1].x - G[nowGi].x) / (nG[nowGi / 2] - 1);
                        if(fabsf(xyCtrl->OptPos.x - G[nowGi].x - passingI * xPreG) < 0.6f)
                        {
                            passingI += 1;
                            if(passingI >= nG[nowGi / 2])
                                stayPointOK = 1;
							if(buttomMV->option != 3)
							{
								laser->enable = 1;
								laser->workTime = 0;
							}
                        }
                    } else {
                        forceSpeedX = 0;
                        forceSpeedY = SIGN(G[nowGi + 1].y - G[nowGi].y);
                        if(stayPointOK)
                            forceSpeedY = 0;
                        yPreG = (G[nowGi + 1].y - G[nowGi].y) / (nG[nowGi / 2] - 1);
                        if(fabsf(xyCtrl->OptPos.y - G[nowGi].y - passingI * yPreG) < 0.6f)
                        {
                            passingI += 1;
                            if(passingI >= nG[nowGi / 2])
                                stayPointOK = 1;
						    
							if(buttomMV->option != 3)
							{
								laser->enable = 1;
								laser->workTime = 0;
							}
                        }
                    }
                }
                if(timeCount >= 100)
                {
                    if(!workingState)
                    {
                        timeCount = 0;
                        stayPointOK = 0;
                        workingState = 1;
						rockerHandX->K = 4;
                    } else {
                        passingI = 0;
                        timeCount = 0;
                        stayPointOK = 0;
                        workingState = 0;
                        nowGi += 2;
						laser->enable = 0;
						rockerHandX->K = 5;
                        forceSpeedX = 0;
                        forceSpeedY = 0;
                        //在去4号点之前先进行坐标修正
                        if(nowGi == 4)
                        {
                            changeToState(comeToPointA);
                            gridInX = 0;
                        } else if(nowGi == 12)
                        {
                            if(demoPPM->data[5] > 1750)
                            {
                                changeToState(findTower);
                            } else {
                                changeToState(landing);
                            }
                            break;
                        }
                    }
                } else if(stayPointOK) {
                    timeCount++;
                }
            break;
        }
    } else {
        timeCount = 0;
    }
	if(pilotState == landed)
		demoPPM->data[4] = 1000;
    
	for(int i = 2; i > 0; i--)
	{
		lastA[0][i] = lastA[0][i - 1];
		lastA[1][i] = lastA[1][i - 1];
	}
	lastA[0][0] = imuRead.gyro.y;
	lastA[1][0] = imuRead.gyro.x;
    PIDctrler_SetKd(&xyCtrl->PID_X_Pos, MAX(demoPPM->data[6] - 1000, 0) / 1000 * 0.2f);
	PIDctrler_SetKd(&xyCtrl->PID_Y_Pos, MAX(demoPPM->data[6] - 1000, 0) / 1000 * 0.2f);

    PIDctrler_SetKp(&xyCtrl->PID_X_Pos, MAX(demoPPM->data[7] - 1000, 0) / 1000 * 1.0f);
	PIDctrler_SetKp(&xyCtrl->PID_Y_Pos, MAX(demoPPM->data[7] - 1000, 0) / 1000 * 1.0f);
    if(demoPPM->data[5] > 1250 && height->h > 0.25f)
    {
        //赛题任务执行部分
        //expDy += eyeMV->midErr / 80.0f;
        //demoAtti.gz = -eyeMV->midErr / 1500.0f;
        RoboHand_UpdateFrame(rockerHandX);
        RoboHand_UpdateFrame(rockerHandY);
        if (flowYTVG->dataLoop)//buttomMV->dataLoop)// && buttomMV->x != 255)
        {
            //位置环
            // 0.5等幅振荡
            //正数向左
            Vector2f expPos = {rockerHandX->var, rockerHandY->var};
            XY_Ctrl_PosUpdateFrame(xyCtrl, xyCtrl->OptPos, expPos);
			if (demoPPM->data[4] > 1500)
			{
                if(forceSpeedX != 0)
                    expDx += forceSpeedX * 1.1f;
                else
				    expDx += xyCtrl->outPut.x;
				
                if(forceSpeedY != 0)
                    expDy -= forceSpeedY * 1.1f;
                else
				    expDy -= xyCtrl->outPut.y;
			}
            // 速度环
            flowYTVG->dataLoop = 0;
            //Vector2f nowXY = {buttomMV->x - 80, buttomMV->y - 120};
			Vector2f nowXY_Speed = {-flowYTVG->optX, -flowYTVG->optY};
            //Vector2f expectXY = {0, 0};
			Vector2f expectXY_Speed = {expDx, -expDy};
			Vector2f nowGyro = {lastA[0][1] * 0.5f, lastA[1][1] * 0.5f};//imuRead.gyro.y * 1121.0f, imuRead.gyro.x * 869.0f};
			Vector2f nowAccel = {imuRead.accel.x, imuRead.accel.y};
            XY_Ctrl_SpeedUpdateFrame_Single(xyCtrl, nowXY_Speed, expectXY_Speed, nowAccel, nowGyro, height);
			//XY_Ctrl_UpdateFrame(xyCtrl, nowXY, expectXY);
            demoAtti.pitch += xyCtrl->outPut.y;
            demoAtti.roll += xyCtrl->outPut.x;
        }
        else if (!flowYTVG->dataLoop)//buttomMV->dataLoop == 0)// && buttomMV->x != 255)
        {
            demoAtti.pitch += xyCtrl->outPut.y;
            demoAtti.roll += xyCtrl->outPut.x;
        }
    }
    else
    {
        XY_Ctrl_Clear(xyCtrl);
        demoAtti.pitch = expDy;
        demoAtti.roll = -expDx;
    }
    /***************************高度控制***************************/
    //PIDctrler_SetKd(&hCtrl->PID_H, MAX(demoPPM->data[5] - 1000, 0) / 1000.0f);
    if (demoPPM->data[4] > 1500)
    {
        if (runningState == 0)
        {
			landHeight = height->h * 0.48f + landHeight * 0.52f;
            RoboHand_SetVarStart(throttleHand, 0);
            RoboHand_SetVarEnd(throttleHand, 1400);
            runningState = 1;
        }
        if (runningState == 1)
        {
            if (height->h <= landHeight + 0.008f && !startLockHeight) //启动，在小于0.07前，由机器人线性手推油门
            {
                RoboHand_UpdateFrame(throttleHand);
                demoAtti.w = throttleHand->var;
            }
            else if (height->h > landHeight + 0.008f && !startLockHeight)
            { //启动，在大于0.07时，无人机已经腾空，启动定高
                runningState = 2;
                midThrottle = demoAtti.w + 200;
                startLockHeight = 1;
                RoboHand_SetVarStart(heightHand, height->h);
            }
        }
        if (startLockHeight) //启动定高，正常飞行控制
        {
            RoboHand_SetVarEnd(heightHand, keepHeight);//MAX(demoPPM->data[7] - 1000, 0) / 750 + 0.2f);
            RoboHand_UpdateFrame(heightHand);
            HeightCtrl_UpdateFrame(hCtrl, height, heightHand->var, midThrottle);
            demoAtti.w = hCtrl->throttle;
        }
    }
    else if (demoPPM->data[4] < 1500)
    {
        if (runningState == 2)
        {
            if (height->h > 0.25f)
            {
                RoboHand_SetVarEnd(heightHand, 0.0f);
                RoboHand_UpdateFrame(heightHand);
                HeightCtrl_UpdateFrame(hCtrl, height, heightHand->var, midThrottle);
                demoAtti.w = hCtrl->throttle;
            }
            else
            {
                runningState = 3;
                HeightCtrl_Clear(hCtrl);
                RoboHand_SetVarStart(throttleHand, midThrottle);
                RoboHand_SetVarEnd(throttleHand, 0);
            }
        }
        if (runningState == 3)
        {
            RoboHand_UpdateFrame(throttleHand);
            demoAtti.w = throttleHand->var;
            if (throttleHand->var <= 500 || height->h < 0.2f)
                runningState = 0;
        }
        if (runningState == 0 || runningState == 1)
        {
            HeightCtrl_Clear(hCtrl);
            demoAtti.w = 0;
            startLockHeight = 0;
			runningState = 0;
			landHeight = height->h * 0.48f + landHeight * 0.52f;
        }
		//没起飞时，在显示屏上显示高度值，防止出现意外
		char hStr[30];
		sprintf(hStr, "Height = %-1.4f    ", height->h);
		display_6_8_string(0, 4, (unsigned char *)hStr);
        char fStr[30];
		sprintf(fStr, "front = %-1.4f    ", frontToF->length);
		display_6_8_string(0, 5, (unsigned char *)fStr);
    }
//	char hStr[30];
//	sprintf(hStr, "Height = %-1.4f    ", height->h);
//	display_6_8_string(0, 4, (unsigned char *)hStr);
    /***************************姿态控制和油门输出***************************/
	
    // PIDctrler_SetKp(&xyCtrl->PID_X_Speed, MAX(demoPPM->data[7] - 1000, 0) / 200 * 0.073407f);
	// PIDctrler_SetKp(&xyCtrl->PID_Y_Speed, MAX(demoPPM->data[7] - 1000, 0) / 200 * 0.073407f);

    // PIDctrler_SetKp(&xyCtrl->PID_X_Speed, MAX(demoPPM->data[5] - 1000, 0) / 200 * 0.073387f);
	// PIDctrler_SetKp(&xyCtrl->PID_Y_Speed, MAX(demoPPM->data[7] - 1000, 0) / 200 * 0.073407f);
	demoAtti.gz += -getPlaneAtti()->yaw / 240.0f;
    AttiCtrl_UpdateFrame(aCtrl, &nowAtti, &demoAtti, Throttle_Get());
    /***************************记录角度积累误差数据***************************/
    // recordLen++;
    // if (demoPPM->data[4] > 1500 && demoPPM->data[5] < 1250 && height->h > 0.7f)
    // {
    //     if(startRecord)
    //     {
    //         if(recordLen >= totalRecordLen)
    //         {
    //             recordLen = totalRecordLen;
    //         } else {
    //             sumAngle[0] += nowAtti.pitch;
    //             sumAngle[1] += nowAtti.roll;
    //         }
    //     } else {
    //         if(recordLen > 800)
    //         {
    //             startRecord = 1;
    //             recordLen = 0;
    //         }
    //     }
    // } else {
    //     startRecord = 0;
    //     recordLen = 0;
    //     sumAngle[0] = 0.0;
    //     sumAngle[1] = 0.0;
    // }
    /********************************激光控制********************************/
    Laser_Process(laser);
    /***************************光流与gyro联合调试***************************/
//    float ampGyro = MAX(demoPPM->data[6] - 1000, 0) / 1000.0f;
//    int phaseGyro = (int)(MAX(demoPPM->data[7] - 1000, 0) / 1000 * 8.0f);
//    float nonBiasFlowX = xyCtrl->lastXY_AccelSpeed.x - ampGyro * lastA[0][phaseGyro];
//    float nonBiasFlowY = xyCtrl->lastXY_AccelSpeed.y - ampGyro * lastA[1][phaseGyro];
    /***************************虚拟示波器输出***************************/
    float VOTdata[8];
	
	// VOTdata[4] = xyCtrl->lastXY_Pos.x;
    // VOTdata[5] = xyCtrl->lastXY_Pos.y;

    // VOTdata[6] = xyCtrl->lastXY_AccelPos.x;
    // VOTdata[7] = xyCtrl->lastXY_AccelPos.y;
    VOTdata[0] = xyCtrl->OptPos.x;
	VOTdata[1] = xyCtrl->OptPos.y;
	VOTdata[2] = buttomMV->option;

	VOTdata[3] = G[nowGi].y + passingI * yPreG;
	// VOTdata[4] = xyCtrl->PID_Y_Speed.out;
    // VOTdata[5] = xyCtrl->PID_Y_Speed.dOut;

    // VOTdata[6] = xyCtrl->lastXY_AccelSpeed.x;
    // VOTdata[7] = xyCtrl->lastXY_AccelSpeed.y;
	// for(int j = 0; j < 7; j++)
	// 	VOTdata[j] = demoPPM->data[j];
    appendData(VOTdata);
    //IMU_ShowRPY(nowAtti);
    Bling_Working(0); //状态指示灯运行
}

/***************************PID参数整定时使用***************************/
void ti_systemInit_PID_tune(void)
{
    System_Start_Init();
    initTime();
    initVOT(1, 8);
    //ADC_Init();
    OLED_Init();
    display_6_8_string(0, 0, (unsigned char *)"Hello FTTfly");
    Init_I2C();
    while (MPU6050_Init())
        ; //MPU6050陀螺仪、加速度计配置初始化
    Atti_Init();
    Throttle_Init(0);
    memset(&demoAtti, 0x00, sizeof(Atti));
    demoAtti.pitch = 0;
    demoAtti.roll = 0;
    demoAtti.yaw = 0;
    aCtrl = AttiCtrl_Init();
    Init_PWM();
    Bling_Init();
    ConfigureUART1();
    PPM_Init();
    butterWorthGyro = ButterWorth_Init(200.0, 40);
    butterWorthAccel = ButterWorth_Init(200.0, 25);
    butterWorthPitch = ButterWorth_Init(200.0, 12);
    Time_init(1); //飞控调度定时器初始化     TODO:y
    delay_ms(200); //初始化延时	     TODO:y
}

void ti_system_Loop_PID_tune(void)
{
    MPU6050_Read_Data(&imuRead.gyro, &imuRead.accel, &imuRead.temperature);
	//imuRead.gyro.x = ButterWorth_Work_LPB(imuRead.gyro.x, &butterWorthGyro_Buf, butterWorthGyro);
    //imuRead.accel.y = ButterWorth_Work_LPB(imuRead.accel.y, &butterWorthAccel_Buf, butterWorthAccel);
    Atti nowAtti = Atti_Update(&imuRead.gyro, &imuRead.accel);
    /***************************遥控器***************************/
    demoPPM = PPM_getData();
    demoAtti.w = (demoPPM->data[2] - 1010) * 2.0f;
	float expDy = (demoPPM->data[1] - 1500) / 500.0f * 30.0f;
    //float expDx = -(demoPPM->data[0] - 1500) / 500.0f * 10.0f;
    demoAtti.pitch = expDy;
    demoAtti.roll = 0;
    //demoAtti.gz = -(demoPPM->data[3] - 1500) / 500.0f * 10.0f / 200.0f;
    //遥控器
    /***************************姿态控制和油门输出***************************/
	PIDctrler_SetKp(&aCtrl->PIDomega[PARAM_PITCH], MAX(demoPPM->data[7] - 1000, 0) / 1000.0f * 3500.0f + 1000);
    PIDctrler_SetKi(&aCtrl->PIDomega[PARAM_PITCH], MAX(demoPPM->data[5] - 1000, 0) / 1000.0f * 5000.0f + 8000.0f);
    AttiCtrl_UpdateFrame(aCtrl, &nowAtti, &demoAtti, Throttle_Get());
    /***************************虚拟示波器输出***************************/
    float VOTdata[8];
    VOTdata[0] = nowAtti.pitch;
    VOTdata[1] = nowAtti.roll;
	
	VOTdata[2] = imuRead.gyro.x;
    VOTdata[3] = imuRead.accel.y;

    VOTdata[4] = aCtrl->PIDomega[PARAM_PITCH].integral;
	
	VOTdata[5] = MAX(demoPPM->data[7] - 1000, 0) / 1000.0f * 3500.0f + 1000;
    VOTdata[6] = MAX(demoPPM->data[5] - 1000, 0) / 1000.0f * 5000.0f + 8000.0f;
	
	VOTdata[7] = demoAtti.w;
    appendData(VOTdata);
    //IMU_ShowRPY(nowAtti);

    Bling_Working(0); //状态指示灯运行
}

uint32_t priority[10];
void nvic_priority_get(void)
{
    priority[0] = IntPriorityGet(INT_TIMER2A);
    priority[1] = IntPriorityGet(INT_TIMER1A);
    priority[2] = IntPriorityGet(INT_TIMER0A);
    priority[3] = IntPriorityGet(INT_GPIOC);
    priority[4] = IntPriorityGet(INT_UART0);
    priority[5] = IntPriorityGet(INT_UART1);
    priority[6] = IntPriorityGet(INT_UART2);
    priority[7] = IntPriorityGet(INT_UART3);
    priority[8] = IntPriorityGet(INT_UART6);
    priority[9] = IntPriorityGet(INT_UART7);
}
